
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       pid_2d.h
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdlib.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#define PID_2D_FILT_HZ_DEFAULT    20.0f    // default input filter frequency
#define PID_2D_FILT_HZ_MIN        0.01f    // minimum input filter frequency
#define PID_2D_FILT_D_HZ_DEFAULT  10.0f    // default input filter frequency
#define PID_2D_FILT_D_HZ_MIN      0.005f   // minimum input filter frequency
/*----------------------------------typedef-----------------------------------*/
typedef struct
{
    float _kp;
    float _ki;
    float _kd;
    float _kff;
    float _kimax;
    float _filt_E_hz;    // PID Input filter frequency in Hz
    float _filt_D_hz;    // D term filter frequency in Hz

    // internal variables
    float      _dt;                // timestep in seconds
    Vector2f_t _integrator;        // integrator value
    Vector2f_t _target;            // target value to enable filtering
    Vector2f_t _error;             // error value to enable filtering
    Vector2f_t _derivative;        // derivative value to enable filtering
    bool       _reset_filter;     // true when input filter should be reset during next call to update_all
}PID_2d_ctrl;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void pid_2d_ctrl_ctor(PID_2d_ctrl * controler, float kp, float ki, float kd, float kff, float kimax, float filt_hz, float filt_d_hz, float initial_dt);

// set_dt - set time step in seconds
static inline void pid_2d_ctrl_set_dt(PID_2d_ctrl * controler, float dt) { controler->_dt = dt; }

//  update_all - set target and measured inputs to PID controller and calculate outputs
//  target and error are filtered
//  the derivative is then calculated and filtered
//  the integral is then updated if it does not increase in the direction of the limit vector
Vector2f_t pid_2d_ctrl_update_all(PID_2d_ctrl * controler, const Vector2f_t* target, const Vector2f_t* measurement, const Vector2f_t* limit);
Vector2f_t pid_2d_update_all2(PID_2d_ctrl * controler, const Vector3f_t *target, const Vector3f_t *measurement, const Vector3f_t *limit);
Vector2f_t pid_2d_update_all3(PID_2d_ctrl * controler, const Vector3f_t *target, const Vector2f_t *measurement, const Vector3f_t *limit);

//  update_i - update the integral
//  If the limit is set the integral is only allowed to reduce in the direction of the limit
void pid_2d_update_i(PID_2d_ctrl * controler, const Vector2f_t *limit);

// get_pi - get results from pid controller
Vector2f_t pid_2d_ctrl_get_pid(PID_2d_ctrl * controler);
Vector2f_t pid_2d_ctrl_get_p(PID_2d_ctrl * controler);
static inline Vector2f_t pid_2d_ctrl_get_i(PID_2d_ctrl * controler) { return controler->_integrator;}
Vector2f_t pid_2d_ctrl_get_d(PID_2d_ctrl * controler);
Vector2f_t pid_2d_ctrl_get_ff(PID_2d_ctrl * controler);
static inline Vector2f_t pid_2d_ctrl_get_error(PID_2d_ctrl * controler) { return controler->_error; }

// reset_I - reset the integrator
void pid_2d_ctrl_reset_I(PID_2d_ctrl * controler);

// reset_filter - input and D term filter will be reset to the next value provided to set_input()
void pid_2d_ctrl_reset_filter(PID_2d_ctrl * controler);

// set accessors
static inline void pid_2d_ctrl_set_kp(PID_2d_ctrl * controler, float kp) { controler->_kp = kp; }
static inline void pid_2d_ctrl_set_ki(PID_2d_ctrl * controler, float ki) { controler->_ki = ki; }
static inline void pid_2d_ctrl_set_kd(PID_2d_ctrl * controler, float kd) { controler->_kd = kd; }
static inline void pid_2d_ctrl_set_kff(PID_2d_ctrl * controler, float kff) { controler->_kff = kff; }
static inline void pid_2d_ctrl_set_kimax(PID_2d_ctrl * controler, float kimax) { controler->_kimax = fabsf(kimax); }
void pid_2d_ctrl_set_filt_E_hz(PID_2d_ctrl * controler, float filt_hz);
void pid_2d_ctrl_set_filt_D_hz(PID_2d_ctrl * controler, float filt_d_hz);

static inline float pid_2d_ctrl_get_kp(PID_2d_ctrl * controler) { return controler->_kp; }
static inline float pid_2d_ctrl_get_ki(PID_2d_ctrl * controler) { return controler->_ki; }
static inline float pid_2d_ctrl_get_kd(PID_2d_ctrl * controler) { return controler->_kd; }
static inline float pid_2d_ctrl_get_kff(PID_2d_ctrl * controler) { return controler->_kff; }
static inline float pid_2d_ctrl_get_kimax(PID_2d_ctrl * controler) { return controler->_kimax; }
static inline float pid_2d_ctrl_get_filt_E_hz(PID_2d_ctrl * controler) { return controler->_filt_E_hz; }
static inline float pid_2d_ctrl_get_filt_D_hz(PID_2d_ctrl * controler) { return controler->_filt_D_hz; }

// get the target filter alpha
static inline float pid_2d_ctrl_get_filt_E_alpha(PID_2d_ctrl * controler) { return math_calc_lpf_alpha_dt(controler->_dt, controler->_filt_E_hz); }

// get the derivative filter alpha
static inline float pid_2d_ctrl_get_filt_D_alpha(PID_2d_ctrl * controler) { return math_calc_lpf_alpha_dt(controler->_dt, controler->_filt_D_hz); }

// integrator setting functions
void pid_2d_set_integrator(PID_2d_ctrl * controler, const Vector2f_t* i);
static inline void pid_2d_set_integrator2(PID_2d_ctrl * controler, const Vector3f_t* i) { Vector2f_t _i = {i->x, i->y}; pid_2d_set_integrator(controler, &_i); }
void pid_2d_set_integrator3(PID_2d_ctrl * controler, const Vector2f_t* error, const Vector2f_t* i);
void pid_2d_set_integrator4(PID_2d_ctrl * controler, const Vector2f_t* target, const Vector2f_t* measurement, const Vector2f_t* i);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



